/******************************************************************************
* AIR SYSTEMS LABORATORY
* AEROSPACE ENGINEERING
* UNIVERISTY OF TEXAS AT AUSTIN
* UAV FLIGHT SOFTWARE 
* DESCRIPTION:
*   
*
* SOURCE: 
* Orignial: 08/14/2008  Jorge Alvarez joroge@gmail.com
* Modified: 03/20/2009	Sergio García-Nieto sergarro@isa.upv.es
* $Id: datalogging_thread.c 4 2009-07-21 17:12:11Z joroge@gmail.com $
*************************************************************************************/
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>

#include "globaldefs.h"
#include "utilities.h"

void *datalogging()
{
	
	struct 	timespec t0,t1,t0_ant,wait,next_activation,WCCT,Period;
	_uint64 auxtime;
	time_t 	time_now;
	struct 	tm *timePtr;
	char 	filestring[100];
	char	FileName1[100]="/flash/Attitude";
	char	FileName2[100]="/flash/Navigation";
	int		fd1,fd2,fd3,fd4;
	int 	nbytes1,nbytes2;
	
	struct timespec delay;
	struct imuShort imuShortBuffer[500];
	struct gpsAdc gpsAdcBuffer[250];
	
	delay.tv_sec=2;
	delay.tv_nsec=0;
	
	// Create the new File Name for Saving Data
	time_now = time(NULL);
	timePtr = gmtime(&time_now);
	strftime(filestring,100,"%b%d_%H:%M",timePtr);
	strcat(FileName1,filestring);
	strcat(FileName2,filestring);
	printf("Datalogging File %s \n",FileName2);
	//printf("Datalogging Step 1 \n");
	// Open Files in HD to save data
	fd1 = open(FileName1,O_WRONLY | O_NONBLOCK | O_CREAT | O_TRUNC | O_APPEND,S_IRWXU | S_IRWXG | S_IRWXO | O_DSYNC | O_REALIDS | O_RSYNC | O_SYNC);
	fd2 = open(FileName2,O_WRONLY | O_NONBLOCK | O_CREAT | O_TRUNC | O_APPEND,S_IRWXU | S_IRWXG | S_IRWXO | O_DSYNC | O_REALIDS | O_RSYNC | O_SYNC);
			
	//printf("Datalogging Step 2 \n");
	// Open de FIFOs
	fd3 = open(fifoAttiDatalog,O_RDONLY | O_NONBLOCK);
	fd4 = open(fifoNavDatalog,O_RDONLY | O_NONBLOCK);
	
	nanosleep(&delay,NULL);
	clock_gettime(CLOCK_REALTIME, &t0_ant);
	printf ("Dataloggin Task on-line.\n");
		
	while(1) 
	{
		//printf("Datalogging Step 3 \n");
		// Loop time claculations
		clock_gettime (CLOCK_REALTIME, &t0);
		next_activation=sum_tiempos(t0,Ts_datalogging); 
		WCCT = diff_tiempos(ID_datalogging,t0_ant,t1,0);
		
		//printf("Datalogging Step 4 \n");
		// Download FIFOs DATA
		//readblock(fd3,sizeof(imuBuffer),0,1,&imuBuffer);
		nbytes1 = read(fd3,&imuShortBuffer,(sizeof(struct imuShort))*250);
		//printf("DATA IMU %3f %3f %ld\n",imuShortBuffer[1].pitch,imuShortBuffer[1].roll,imuShortBuffer[1].counter);
		nbytes2 = read(fd4,&gpsAdcBuffer,(sizeof(struct gpsAdc))*100);
		//printf("DATA GPS %3f %3f \n",gpsAdcBuffer[1].lat,gpsAdcBuffer[1].lon);
		
		//printf("Datalogging Step 5 \n");
		// Saving in HDD
		writeblock(fd1,sizeof(struct imuShort),0,(nbytes1/sizeof(struct imuShort)),&imuShortBuffer);
		writeblock(fd2,sizeof(struct gpsAdc),0,(nbytes2/sizeof(struct gpsAdc)),&gpsAdcBuffer);
		//printf("FIFO READ Atti: %d\n",(nbytes1/sizeof(struct imuShort)));
		//printf("FIFO READ Nav: %d\n",(nbytes2/sizeof(struct gpsAdc)));
		// Print the results
		//if(v) printf("Inicialization--> WCCT: %3d Period: %3d\n",((int)WCCT.tv_nsec)/prec,  ((int)t0.tv_nsec-(int)t0_ant.tv_nsec)/prec);
		//if(v) printf("Datalogging Fin %3d %3d \n",((int)WCCT.tv_nsec)/prec,  ((int)Period.tv_nsec)/prec);
		// Wait until the next activation
		Period=diff_tiempos(ID_datalogging,t0,t0_ant,0);
		t0_ant=t0;
		clock_gettime (CLOCK_REALTIME, &t1);
		if (timespec2nsec(&next_activation)>timespec2nsec(&t1))
		{
		    auxtime = (timespec2nsec(&next_activation)-timespec2nsec(&t1));
			nsec2timespec(&wait,auxtime);
			nanosleep(&wait, NULL);
		}
	}
	//printf("Datalogging Step 6 \n");
	// Close FIFOs
	close(fd3);
	close(fd4);
	
	//printf("Datalogging Step 7 \n");
	// Close HDD Files
	close(fd1);
	close(fd2);
	
	return(NULL);
}